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Abstract 

This paper examines maximizing the speed of movement, along a prescribed path, of the system 
formed by a set of robot arms and the object they hold. The actuator torques that maximize the 
acceleration of the system are shown to be determined by the solution to a standard linear 
programming problem. The combination of this result with the known control strategy for time 
optimal movement of a single robot arm yields an algorithm for time optimal movement of multiple 
robot arms holding the same workpiece. 


The problem of controlling the movement of an actuated closed kinematic chain is a model for 
coordinating movement of several arms holding the same object, Luh and Zheng 1985, Tarn* © : • 
1987, manipulating an object with the fingers of a mechanical hand, Salisbury and Craig 1982, Li et 
al. 1988, and controlling the posture of a walking machine, Orin and Oh 1981. In each case an 
important benefit of the closed chain is the distribution of the load at manipulated body, or 
workpiece, over the actuators of several different robots. 


The problem of specifying the joint torques to achieve a specific movement of the chain is 
underdetermined, which means a variety of joint torque histories perform the same movement. 
Recent research focusses on using this freedom to balance the load among the actuators by 
minimizing the total power consumed by the system along the trajectory, Kreutz and Lokshin 1988 
I .nh and Zheng 1988 and Zheng and Luh 1988. 


In this paper the dynamic indeterminacy that appears in the control of two cooperating 3R robots is 
resolved by seeking the joint torque history that achieves the least transit time along a specified path. 
This result generalizes the known solution for the open chain case, Bobrow et al. 1985, and joins 
minimum time path planning research for closed chains with the existing effort for open chains, 
Gilbert and Johnson 1985, Shiller and Dubowsky 1988, Bobrow 1988, and Rajan 1985. 


Dynamics Equations for Cooperating 3R Robots 

In this section the equations of motion for the closed chain formed by a pair of cooperating 
man ipulators are derived, focussing on the case of two planar robots. Pwrious work in this area 
includes the studies of the cooperation of walking machine legs Orin and Oh 1981 , of dual arm 
robots Luh and Zheng 1985, and Tam et al. 1987, Kreutz and Lokshin 1988, and of mechanical 
hands, Nakamura et al. 1986 and Li et al. 1988. 

In this derivation we follow Kreutz and Lokshin 1988 and assume that the end effectors of each 
robot rigidly hold the workpiece. The combination of these end-effectors and the workpiece is 
viewed as a single moving rigid body. The joints at each end effector provide the forces and 
moments that move this body. This viewpoint allows us to formulate the dynamics of each arm and 
the workpiece independendy. 



Let the two robots be denoted Robot 1 and Robot 2. The joints of Robot 1 arc defined by the 
position vectors oj, ai, and bi, similarly the joints of Robot 2 are 02 , 82 , and b 2 , the rotation 

angles at each of these joints are denoted 0J, <pj, and \|fj, i=l*2, respectively. See Fig. 1. The 
lengths the first two links of each robot are Kplaj-Ojl and Lj=lbj-a;l, i=l,2. The moving body is 
the rigid link connecting the end joints bj and b 2 , its length is H. The distance between the base 
joints 01 and 02 is G. For convenience assume that each link is uniform so that its center of mass 
lies halfway between the joints it contains. 



Figure 1. A pair of 3R planar robots holding the same workpiece. 


Dynamics of the workpiece. Let x=(x,y) T be the position of the center of mass of the 

workpiece and a be its orientation. If fi=(Xi,Y i)T and f 2 =(X 2 , Y 2 )^ are the forces of interaction at 

the joints bj and b 2 and 'Fj and 'F 2 be the motor torques, then the equations of motion of the 
workpiece arc: 


mx = f , + f „ 

1 2 » 

ia= V f i + V f 2 + 


where 


( 1 ) 


=cr(H/2sin a, - H/2cosa) T ,i = 1,2 , 

*®d 0=1 for Robot 1. and o=-l for Robot 2. The vectors aj determine the moment of the joint forces 
about the center of mass. The dot denotes differentiation with respect to time, and m is the mass of 
the workpiece while I is its moment of inertia about the center of mass. 

Eq. (1) can be written in matrix form by adding a to the coordinate vector x, so that 
x = (x,y,a) and we obtain 
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( 2 ) 


[M O l i = A l , ! + A ! , ! + '0 ’ 

The torque vector is obtained from (1) as 


To=(0,°,'V'J' 2 ) T (3) 

and Aj and A2 are 3x2 arrays consisting of the 2x2 identity matrix for the first two rows and the 
vector a; in the third row. 

The dynamics of each robot. Since the workpiece and end effectors of the robots have been 
combined into a single body, the equations of motion of each robot reduce to those of a doub e 

pendulum with forces applied at the end point bj. Assemble the joint angles into the vector 6j=(0i, 

<Pj)T, and let 0j, Oj be associated joint torques. The equations of the two robots can be written as 

P ,1 i( e i)]®i + h i( 8 i’®i) = t i ~ C i' i = U W 


where 


x r( B r v v <s> r' v i) T - 


(5) 


[Mj] is the 2x2 mass matrix, hj is the vector of Coriolis and gravitational terms, and Cj arises from 
the interaction forces at the workpiece. The form of these terms is the same for both robots so we 
drop the subscript notation: 


[ 1 ^( 0 )] = 

I K + I L +m K (K/2) 2 + m L (L/2) 2 + m L K 2 + m L LKcos <p I L +m l <L/2) 2 + |m L LKcos<p 
I L + m L (L/2) 2 + ^m L LKcos9 I L +m L (L/2) 

( 6 ) 


h.(0,0) = 

-i(20+ <p)<pm L LKsin <p-^(m K + 2m L )gKcos0 -m L g(L/2)cos(0+ <p) 


2 

v -i-0 m L LK sin 9 - m L g(L/2)cos(0 + 9) 


(7) 


where the superscripts K, L refer to the cooresponding link with these lentghs. 

The vectors Cj i=l,2 are the generalized joint torques associated with the forces fj=(Xj,Yj) exerted 
at bj, and are given by the equations 

Cj = [Ji T ] fj, i=l,2, ( g ) 
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where 



-K.se.- L.s(e. + <p.) - Li s(e. +q>.)- , 

K i ce i +L i c ( e i + < f > i) l jC(e . + <Pj) 


(9) 


is the Jacobian of the i^ two link chain. Note s and c denote the sine and cosine functions. 

The dynamics equations of the workpiece and of the two robots are coupled by the interaction forces 
at bi. 


The constraint equations. The coordinates, x, y, and a, of the workpiece and the joint angles, 
®i* 9i> Vi> i = l»2, are related by the kinematics equations of the two robots: 

x = -oG + K . cos 0 . + L. cos(0 . + <p.) + o(H/2)cos a, 
y = K . sin 0 . + L . sin (0 . + q>.) + a(H/2)sin a, i = l,2 ^ 

where a=l for Robot 1 and o=-l for Robot 2. Given values for x=(x, y, a), (10) can be solved to 
determine each of the coordinate vectors 0|=(0j, <pj), i=l,2. 

We now determine the relation between the joint velocities 0 ^ -(* ., <j>.) and accelerations 0., 

and die velocity and acceleration, x, x, of the workpiece. This conveniently done by introducing the 
2 dimensional vector 


x = (x + oG -<j(H/2)cosa, Y -o(H/2)sin a) 
The derivative of (10) can now be written as 


and 


« = [Ji]0 r i= U 


( 11 ) 


( 12 ) 


(13) 


where [J.] is the Jacobian of each robot, Eq. (9), and^j. J is its derivative with respect to time. For 
a given position, velocity and acceleration, x, x, x of the workpiece, we can compute x, x, x , and 


solve equations (12) and (13) to determine 0 and 0 . . 

i i 


Time Optimal Control 

The problem of controlling the cooperating robot pair so that the workpiece traverses a specified path 
m minimum time is a generalization of work presented in Bobrow et al. 1985. Also see Dubowskv 
and Shiller 1988 and Shin and McKay 1984. 
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First, we assemble the equations of motion of the workpiece and the two arms into a single set of 

T 

equations by introducing the seven dimensional vector q = ( x, 0 1? 0 2 ) --note x has three 
components and 0 . each have two components. The equations of motion of the system become 

[Mlq + h(q,q) = [B]x - [C]f (14) 

The 7x7 system mass matrix [M] has [M J , [M J and [M J along its diagonal. 


The vector h( q, q) is simply h = (0, 0, 0, hj, h 2 )^ obtained from (7). The system torque vector 

T 

x=(@ r Oj, 'F r © r <1> 2 , ¥ 2 ) is six dimensional and [B] is the 7x6 matrix that maps it 

to the three torque vectors Tj, x 2 : 


[B] = 


0 


0 

0 

1 

0 

0 

0 


0 0 

0 0 

0 1 

0 -1 
1 -1 
0 0 

0 0 


0 0 0 

0 0 0 

0 0 1 

0 0 0 

0 0 0 

1 0 -1 

0 1 -1 


(15) 


The four dimensional vector f=(fi,f2) T represents the interaction forces at the workpiece. The 7x4 
matrix [C] is obtained from Eqs. (2) and (8) as 



J 


1 


1 

T 


0 



(16) 


Note that Ai and A 2 are 3x2 arrays, while Ji^ and J2^ are 2x2 arrays. 

The next step is to introduce a path parameter s which identifies the position of the workpiece as it 
moves along the specified trajectory. The system equations of motion (14) arc written in terms of 

this parameter. The goal is to determine the maximum acceleration s along the path that is 
achievable without exceeding the torque limits of the joint motors. 


The path parameter. Since it is assumed a path has been specified, the parameterized vector x(s) 
= (x(s), y(s), a(s)) T is a known function of some parameter s. We seek the function s(t) that 
minimize s the transit time without exceeding the maximum torque attainable at each joint Since x(s) 
is given, we can determine x( s) from (11), and obtain its derivatives in the form 
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k = (dX/ds)s 

X = (d 2 2/ds 2 )s 2 +(dS:/ds)s . (17) 

Using Eqs. (12) and (13) we obtain 

ej'Pj'Wds)* 

Ultimately, we obtain q = (x, § t , § 2 ) in the form 
Q = sq (s, s)+q,(s, s) , 

(19) 

where q ^s, s) is the vector of elements that multiplies s and q .(s, s) is the vector of remaining 
elements. 

The equations of motion (14) can now be written in terms of the path parameter, s, as: 

[MlqjS + g(q,q) = [B]x - [C]f, 

( 20 ) 

where 


g(q>q) = [M]q 2 +h(q,q). 

For given values of s and s , Eq. (20) becomes a set of seven linear equations in the eleven 
unknowns s , x , and f. 


The linear programming problem. The optimal control problem now reduces to computing the 
torques t = (©j, <X>j, 'Pj, 0^ that provide the maximum (or minimum ) 

acceleration s for each position and velocity, (s, s) , of the workpiece, subject to the constraint that 
the equations of motion (20) are satisfied. Note that because the cooperating robots form a three 

degree of freedom system (7 coordinates - 4 constraints), for a given acceleration s the torques are 
not in general unique. 


The problem of maximizing the acceleration can be posed as a standard problem in Linear 

Programming, Thie 1979. That is, a vector y = (s, x, f) T is sought that maximizes (or 
m in i mizes) the scalar acceleration 


s = by, b = (1,0,0, a 0,0,(X a 0,0,0) 

The vector y is subject to the linear constraints due to the equations of motion 


( 21 ) 
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[ A] y = c, 

where from (20) we have 


( 22 ) 


[A] =[[Mlq 1 ,— [B],[C]] (23 ) 

c=-g. 

Bounds on the magnitudes of the components of y are also part of the standard linear programming 
problem, which is how torque limits on the motors are introduced. Notice that we can also introduce 
bounds on the magnitude of the forces acting on the workpiece. The arrays [A], and c are known 
constants for every position and velocity (s, s) , so a standard Linear Programming algorithm can be 
used to determine y that maximizes s, in (21) subject to the constraints in (22). 

The solution of this problem provides the set of joint torques x as well as the interaction forces fi 
and f 2 that yield extreme values for the acceleration, s, of the workpiece, for each point (s, s) in 
phase space. The result is the ability to compute the acceleration bounds f(s,s) and g(s,s), 

f(s,s) <, s < g(s,s) > (24) 

such that the dynamics equations (20) are satisfied. 


The control strategy. With the ability to compute the maximum and minimum accelerations 
attainable by the cooperating robot system, the control strategy established in Bobrow et al. 1985 can 
be applied to achieve a time optimal movement The essential idea is to always drive the system at its 
maximum acceleration or deceleration. From this point of view, the problem reduces to the 
computation of the points at which the shift from acceleration to deceleration and back again occur 
along the path. To find these switching points, the acceleration equation, 

s=g(s,s) > (25) 

is integrated forward in time from the initial position, and the deceleration equation, 

s=f(s,s) f (26) 

is integrated backward in time from the final position to find when they intersect in phase space. 

This intersection determines the value of s along the path at which the controller shifts from 
acceleration to deceleration. In phase space the relation f(s, s) = g(s, s) defines a curve that 
represents the maximum velocity attainable by the workpiece. It can happen that this maximum 
velocity constraint is violated before the intersection of the solutions to (25) and (26) occurs. 

Bobrow et al. 1985 show how to determine intermediate switching points when this occurs, so that 
time optimal movement is maintained. 


Conclusion 

In this paper, the time optimal control problem is formulated for the case of two cooperating planar 
3R robots. The control strategy is shown to be a generalization of the time optimal control of a single 
robot arm. The problem of finding minimum time paths for the cooperating 3R arms is a similar 
generalization of the problem of finding minimum time trajectories for single robot arms. Given the 
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ability to measure transit times for various paths between two desired positions of the workpiece held 
by several robots, a nonlinear optimization routine can vary the trajectory, while avoiding obstacles 
in configuration space, until the minimum time path is found. 
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